import numpy as np
import sys
import os
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + "/util")
from Util import AlgorithmUtil
import time


class MeasureAlgorithm:
    # 计算轨迹的单向改进豪斯多夫距离
    @staticmethod
    def onewayHausdorff(trajectory_A, trajectory_B):
        # 计算得到投影后的轨迹、轨迹长度、轨迹段长度列表
        projectedTrajectory_A, totalLen_A, lenList_A = AlgorithmUtil.getTrajectoryLen(
            trajectory_A
        )
        projectedTrajectory_B, totalLen_B, lenList_B = AlgorithmUtil.getTrajectoryLen(
            trajectory_B
        )
        # 计算轨迹A在轨迹B上的匹配轨迹，以及其Hausdorff距离
        # 轨迹A的累计长度
        accLen_A = 0
        # 创建一个指针指向轨迹B中的每一个轨迹段，以寻找到对应段
        arrow_B = 0
        # 创建一个变量，用于存储计算得到的轨迹B中的轨迹点与轨迹B起点的累积距离
        accLen_B = lenList_B[0]
        # 存储匹配点间距离的列表
        h_A_B = []
        # 遍历轨迹A中的每一个轨迹点
        for i, point_A in enumerate(projectedTrajectory_A):
            # 如果是第一个轨迹点
            if i == 0:
                # 匹配点为另一条轨迹的第一个点
                h_A_B.append(
                    np.linalg.norm(
                        np.array(projectedTrajectory_A[0])
                        - np.array(projectedTrajectory_B[0])
                    )
                )
            # 如果是最后一个轨迹点
            elif i == len(projectedTrajectory_A) - 1:
                # 匹配点为另一条轨迹的最后一个点
                h_A_B.append(
                    np.linalg.norm(
                        np.array(projectedTrajectory_A[len(projectedTrajectory_A) - 1])
                        - np.array(
                            projectedTrajectory_B[len(projectedTrajectory_B) - 1]
                        )
                    )
                )
            # 其他轨迹点
            else:
                # 计算轨迹A中轨迹点与轨迹A起点的累积距离
                accLen_A = accLen_A + lenList_A[i - 1]
                # 计算轨迹点当前在轨迹A中的位置比例
                locateRatio_A = accLen_A / totalLen_A
                # 依据位置比例计算出对应点与轨迹B起点的累积距离
                accLen_A_in_B = totalLen_B * locateRatio_A
                while arrow_B < len(lenList_B):
                    # 找到对应段
                    if accLen_B >= accLen_A_in_B:
                        # 找到这个轨迹段长度
                        targetLen = lenList_B[arrow_B]
                        # 计算长度差比例
                        len_ratio = (accLen_B - accLen_A) / targetLen
                        # 计算对应点坐标
                        x_diff = (
                            projectedTrajectory_B[arrow_B + 1][0]
                            - projectedTrajectory_B[arrow_B][0]
                        )
                        y_diff = (
                            projectedTrajectory_B[arrow_B + 1][1]
                            - projectedTrajectory_B[arrow_B][1]
                        )
                        couple_coord = [
                            projectedTrajectory_B[arrow_B + 1][0] - x_diff * len_ratio,
                            projectedTrajectory_B[arrow_B + 1][1] - y_diff * len_ratio,
                        ]
                        # 计算与对应点以及匹配轨迹段端点的欧氏距离,并取其最小值
                        minDis = np.min(
                            [
                                np.linalg.norm(
                                    np.array(projectedTrajectory_A[i])
                                    - np.array(projectedTrajectory_B[arrow_B])
                                ),
                                np.linalg.norm(
                                    np.array(projectedTrajectory_A[i])
                                    - np.array(projectedTrajectory_B[arrow_B + 1])
                                ),
                                np.linalg.norm(
                                    np.array(projectedTrajectory_A[i])
                                    - np.array(couple_coord)
                                ),
                            ]
                        )
                        h_A_B.append(minDis)
                        break
                    else:
                        arrow_B = arrow_B + 1
                        accLen_B = accLen_B + lenList_B[arrow_B]
        return np.average(h_A_B)

    # 双向改进豪斯多夫距离
    @staticmethod
    def Hausdorff(trajectory_A, trajectory_B):
        return np.max(
            [
                MeasureAlgorithm.onewayHausdorff(trajectory_A, trajectory_B),
                MeasureAlgorithm.onewayHausdorff(trajectory_B, trajectory_A),
            ]
        )

    # 计算轨迹向量的相似性
    @staticmethod
    def simVec(vec_A, vec_B, theta):
        # 向量A和向量B的余弦距离以及夹角
        cosDis, angle = AlgorithmUtil.cosinDistance(vec_A, vec_B)
        # 计算夹角余弦值
        cosin_A_B = 1 - cosDis
        cosine = np.cos(np.radians(theta))
        if cosin_A_B > cosine:
            return cosin_A_B
        else:
            return 0

    # 计算轨迹的空间方向相似性LCSS
    @staticmethod
    def LCSS(trajectory_A, trajectory_B, theta=45):
        # 获取轨迹的方向向量序列
        vecList_A, angleList_A = AlgorithmUtil.caculateVector(trajectory_A)
        vecList_B, angleList_B = AlgorithmUtil.caculateVector(trajectory_B)
        # 构建一个(vecList_A.len+1)*(vecList_B.len+1)的零矩阵
        DP_matrix = np.zeros(shape=(len(vecList_A) + 1, len(vecList_B) + 1))
        # 遍历矩阵中的每一项，执行动态规划算法
        for i in range(1, DP_matrix.shape[0]):
            for j in range(1, DP_matrix.shape[1]):
                DP_matrix[i, j] = np.max(
                    [
                        DP_matrix[i - 1, j],
                        DP_matrix[i, j - 1],
                        DP_matrix[i - 1, j - 1]
                        + MeasureAlgorithm.simVec(
                            vecList_A[i - 1], vecList_B[j - 1], theta
                        ),
                    ]
                )
        # 计算匹配长度
        pointer = [DP_matrix.shape[0] - 1, DP_matrix.shape[1] - 1]
        matchedCount = 0
        while DP_matrix[pointer[0], pointer[1]] != 0:
            # 判断是否与左侧元素相同
            if (
                DP_matrix[pointer[0], pointer[1]]
                == DP_matrix[pointer[0] - 1, pointer[1]]
            ):
                # 指针移动
                pointer = [pointer[0] - 1, pointer[1]]
            # 判断是否与上侧元素相同
            elif (
                DP_matrix[pointer[0], pointer[1]]
                == DP_matrix[pointer[0], pointer[1] - 1]
            ):
                # 指针移动
                pointer = [pointer[0], pointer[1] - 1]
            # 判断是否来自斜上方元素
            elif DP_matrix[pointer[0], pointer[1]] == DP_matrix[
                pointer[0] - 1, pointer[1] - 1
            ] + MeasureAlgorithm.simVec(
                vecList_A[pointer[0] - 1], vecList_B[pointer[1] - 1], theta
            ):
                # 指针移动
                pointer = [pointer[0] - 1, pointer[1] - 1]
                matchedCount = matchedCount + 1
        # 计算相似性
        simTr = (
            2
            * matchedCount
            / (len(vecList_A) + len(vecList_B))
            * DP_matrix[-1, -1]
            / np.min([len(vecList_A), len(vecList_B)])
        )
        # 打印
        # print(DP_matrix,DP_matrix[-1,-1],match_count,simTr)
        # 返回距离
        return 1 - simTr


if __name__ == "__main__":
    A = [[0, 1], [1, 4]]
    B = [[0, 1], [1, 1]]
    startTime = time.time()
    # print(MeasureAlgorithm.Hausdorff(A, B))
    print(MeasureAlgorithm.LCSS(A, B,180))
    endTime = time.time()
    print(endTime - startTime)
